/**
  ******************************************************************************
  * @file    pid.c
  * @author  GEEKROS,  site:www.geekros.com
  ******************************************************************************
*/

#include "pid.h"

/******************************************************************
  * @ 函数名  ： Pid_Init
  * @ 功能说明： PID初始化
  * @ 参数    ： NULL
  * @ 返回值  ： NULL
  *****************************************************************/
void Pid_Init(Pid_Struct *pid, float kp, float ki, float kd)
{
	pid->kp = kp;
	pid->ki = ki;
	pid->kd = kd;
}

/******************************************************************
  * @ 函数名  ： Pid_Dog_Calculation
  * @ 功能说明： 机器狗PID计算
  * @ 参数    ： NULL
  * @ 返回值  ： NULL
  *****************************************************************/
float Pid_Dog_Calculation(float target, float current)
{
	Pid_Struct p = {0};
	p.kp = 0.8;
	p.ki = 0;
	p.kd = 0;
	p.error = target - current;
	p.output = p.kp * p.error;
	current = current + p.output;
	return current;
}
